#include "messages.h"
#include <string.h>

BYTE Calc8CRC(int length, void *VoidBuffer)
{
  int i;
  BYTE CRC=0;
  BYTE CRCOld=0;
  BYTE *Buffer=(BYTE *)VoidBuffer;

  for(i=0; i<length; i++)
  {
    CRCOld=CRC;
    CRC=CRC+Buffer[i]; 
    if(CRC<CRCOld)
    {
      CRC++;
    }
  }

  return CRC;
}

void MirrorBuffer(int length, void *VoidBuffer)
{
  int i;
  BYTE temp;
  BYTE *Buffer=(BYTE*)VoidBuffer;

  for(i=0; i<length/2; i++)
  {
    temp=Buffer[i];
    Buffer[i]=Buffer[length-i-1];
    Buffer[length-i-1]=temp;
  }
}

void CompressMessage1(Message1 *Mes1, CompMessage1 *CompMes1)
{
  memset(CompMes1, 0, sizeof(CompMessage1));

  // Num
  CompMes1->Buffer[0]+=((Mes1->Num)<<4) & 0xf0;

  // Time
  CompMes1->Buffer[0]+=((Mes1->Tst)>>12) & 0x0f;
  CompMes1->Buffer[1]+=((Mes1->Tst)>>4) & 0xff;
  CompMes1->Buffer[2]+=((Mes1->Tst)<<4) & 0xf0;

  //Ny
  CompMes1->Buffer[2]+=((Mes1->Ny)>>2) & 0x0f;
  CompMes1->Buffer[3]+=((Mes1->Ny)<<6) & 0xc0;

  //Ki
  CompMes1->Buffer[3]+=(BYTE)((Mes1->Ki)>>14) & 0x3f;
  CompMes1->Buffer[4]+=(BYTE)((Mes1->Ki)>>6) & 0xff;
  CompMes1->Buffer[5]+=(BYTE)((Mes1->Ki)<<2) & 0xfc;

  //C2
  CompMes1->Buffer[5]+=((Mes1->C2)>>5) & 0x03;
  CompMes1->Buffer[6]+=((Mes1->C2)<<3) & 0xf8;

  //C3
  CompMes1->Buffer[6]+=((Mes1->C3)>>8) & 0x07;
  CompMes1->Buffer[7]+=((Mes1->C3)) & 0xff;

  //C4
  CompMes1->Buffer[8]+=((Mes1->C4)>>3) & 0xff;
  CompMes1->Buffer[9]+=((Mes1->C4)<<5) & 0xe0;

  //PB
  CompMes1->Buffer[9]+=((Mes1->PB)<<4) & 0x10;

  CompMes1->CRC=Calc8CRC(sizeof(CompMes1->Buffer), CompMes1->Buffer);
}

void CompressMessage2(Message2 *Mes2, CompMessage2 *CompMes2)
{
  memset(CompMes2, 0, sizeof(CompMessage2));

  //Num
  CompMes2->Buffer[0]+=((Mes2->Num)<<4) & 0xf0;

  //Kbd
  CompMes2->Buffer[0]+=((Mes2->Kbd)>>1) & 0x0f;
  CompMes2->Buffer[1]+=((Mes2->Kbd)<<7) & 0x80;

  //Ksd
  CompMes2->Buffer[1]+=((Mes2->Ksd)<<2) & 0x7c;
  
  //t1
  CompMes2->Buffer[1]+=(BYTE)((Mes2->T1)>>24) & 0x03;
  CompMes2->Buffer[2]+=(BYTE)((Mes2->T1)>>16) & 0xff;
  CompMes2->Buffer[3]+=(BYTE)((Mes2->T1)>>8) & 0xff;
  CompMes2->Buffer[4]+=(BYTE)((Mes2->T1)) & 0xff;

  CompMes2->CRC=Calc8CRC(sizeof(CompMes2->Buffer), CompMes2->Buffer);
}

void CompressMessage3(Message3 *Mes3, CompMessage3 *CompMes3)
{
  memset(CompMes3, 0, sizeof(CompMessage3));

  //Num
  CompMes3->Buffer[0]+=((Mes3->Num)<<4) & 0xf0;
  
  //alpha
  CompMes3->Buffer[0]+=((Mes3->Alpha)>>9) & 0x0f;
  CompMes3->Buffer[1]+=((Mes3->Alpha)>>1) & 0xff;
  CompMes3->Buffer[2]+=((Mes3->Alpha)<<7) & 0x80;

  //ti
  CompMes3->Buffer[2]+=(BYTE)((Mes3->Ti)>>13) & 0x7f;
  CompMes3->Buffer[3]+=(BYTE)((Mes3->Ti)>>5) & 0xff;
  CompMes3->Buffer[4]+=(BYTE)((Mes3->Ti)<<3) & 0xf8;

  //P
  CompMes3->Buffer[4]+=((Mes3->P)>>3) & 0x07;
  CompMes3->Buffer[5]+=((Mes3->P)<<5) & 0xe0;
  
  //tau
  CompMes3->Buffer[5]+=((Mes3->Tau)>>5) & 0x1f;
  CompMes3->Buffer[6]+=((Mes3->Tau)<<3) & 0xf8;

  //CI
  CompMes3->Buffer[6]+=((Mes3->CI)<<1) & 0x06;

  //fn
  CompMes3->Buffer[6]+=((Mes3->fn)>>12) & 0x01;
  CompMes3->Buffer[7]+=((Mes3->fn)>>4) & 0xff;
  CompMes3->Buffer[8]+=((Mes3->fn)<<4) & 0xf0;

  //dF
  CompMes3->Buffer[8]+=((Mes3->dF)>>6) & 0x0f;
  CompMes3->Buffer[9]+=((Mes3->dF)<<2) & 0xfc;

  //df
  CompMes3->Buffer[9]+=((Mes3->df)>>13) & 0x03;
  CompMes3->Buffer[10]+=((Mes3->df)>>5) & 0xff;
  CompMes3->Buffer[11]+=((Mes3->df)<<3) & 0xf8;

  // kfm
  CompMes3->Buffer[11]+=((Mes3->Kfm)) & 0x07;

  // Nck
  CompMes3->Buffer[12]+=((Mes3->Nck)>>6) & 0xff;
  CompMes3->Buffer[13]+=((Mes3->Nck)<<2) & 0xfc;

  // kam
  CompMes3->Buffer[13]+=((Mes3->Kam)>>1) & 0x03;
  CompMes3->Buffer[14]+=((Mes3->Kam)<<7) & 0x80;

  //kcm
  CompMes3->Buffer[14]+=((Mes3->Kcm)<<4) & 0x70;

  CompMes3->CRC=Calc8CRC(sizeof(CompMes3->Buffer), CompMes3->Buffer);
}

void CompressMessage4(Message4 *Mes4, CompMessage4 *CompMes4)
{
  memset(CompMes4, 0, sizeof(CompMessage4));

  //Num
  CompMes4->Buffer[0]+=((Mes4->Num)<<4) & 0xf0;
  
  //alpha
  CompMes4->Buffer[0]+=((Mes4->Alpha)>>9) & 0x0f;
  CompMes4->Buffer[1]+=((Mes4->Alpha)>>1) & 0xff;
  CompMes4->Buffer[2]+=((Mes4->Alpha)<<7) & 0x80;

  //ti
  CompMes4->Buffer[2]+=(BYTE)((Mes4->Ti)>>13) & 0x7f;
  CompMes4->Buffer[3]+=(BYTE)((Mes4->Ti)>>5) & 0xff;
  CompMes4->Buffer[4]+=(BYTE)((Mes4->Ti)<<3) & 0xf8;

  //P
  CompMes4->Buffer[4]+=((Mes4->P)>>3) & 0x07;
  CompMes4->Buffer[5]+=((Mes4->P)<<5) & 0xe0;
  
  //tau
  CompMes4->Buffer[5]+=((Mes4->Tau)>>5) & 0x1f;
  CompMes4->Buffer[6]+=((Mes4->Tau)<<3) & 0xf8;

  //CI
  CompMes4->Buffer[6]+=((Mes4->CI)<<1) & 0x06;

  //fn
  CompMes4->Buffer[6]+=((Mes4->fn)>>12) & 0x01;
  CompMes4->Buffer[7]+=((Mes4->fn)>>4) & 0xff;
  CompMes4->Buffer[8]+=((Mes4->fn)<<4) & 0xf0;

  //dF
  CompMes4->Buffer[8]+=((Mes4->dF)>>6) & 0x0f;
  CompMes4->Buffer[9]+=((Mes4->dF)<<2) & 0xfc;

  //df
  CompMes4->Buffer[9]+=((Mes4->df)>>13) & 0x03;
  CompMes4->Buffer[10]+=((Mes4->df)>>5) & 0xff;
  CompMes4->Buffer[11]+=((Mes4->df)<<3) & 0xf8;

  // kfm
  CompMes4->Buffer[11]+=((Mes4->Kfm)) & 0x07;

  // Nck
  CompMes4->Buffer[12]+=((Mes4->Nck)>>6) & 0xff;
  CompMes4->Buffer[13]+=((Mes4->Nck)<<2) & 0xfc;

  // kam
  CompMes4->Buffer[13]+=((Mes4->Kam)>>1) & 0x03;
  CompMes4->Buffer[14]+=((Mes4->Kam)<<7) & 0x80;

  //kcm
  CompMes4->Buffer[14]+=((Mes4->Kcm)<<4) & 0x70;

  //f1
  CompMes4->Buffer[14]+=((Mes4->f1)>>9) & 0x0f;
  CompMes4->Buffer[15]+=((Mes4->f1)>>1) & 0xff;
  CompMes4->Buffer[16]+=((Mes4->f1)<<7) & 0x80;

  //f2
  CompMes4->Buffer[16]+=((Mes4->f2)>>6) & 0x7f;
  CompMes4->Buffer[17]+=((Mes4->f2)<<2) & 0xfc;

  //f3
  CompMes4->Buffer[17]+=((Mes4->f3)>>11) & 0x03;
  CompMes4->Buffer[18]+=((Mes4->f3)>>3) & 0xff;
  CompMes4->Buffer[19]+=((Mes4->f3)<<5) & 0xe0;

  //f4
  CompMes4->Buffer[19]+=((Mes4->f4)>>8) & 0x1f;
  CompMes4->Buffer[20]+=((Mes4->f4)) & 0xff;

  //f5
  CompMes4->Buffer[21]+=((Mes4->f5)>>6) & 0xff;
  CompMes4->Buffer[22]+=((Mes4->f5)<<3) & 0xf8;


  CompMes4->CRC=Calc8CRC(sizeof(CompMes4->Buffer), CompMes4->Buffer);
}

void DeCompressMessage1(Message1 *Mes1, CompMessage1 *CompMes1)
{
  DWORD temp=0;
  memset(Mes1, 0, sizeof(Message1));

   // Num
  temp=CompMes1->Buffer[0] & 0xf0;
  Mes1->Num=(BYTE)temp>>4;

  // Time
  temp=(CompMes1->Buffer[0]) & 0x0f;
  Mes1->Tst=(WORD)temp<<12;
  temp=(CompMes1->Buffer[1]) & 0xff;
  Mes1->Tst+=(BYTE)temp<<4;
  temp=(CompMes1->Buffer[2]) & 0xf0;
  Mes1->Tst+=(BYTE)temp>>4;

  //Ny
  temp=(CompMes1->Buffer[2]) & 0x0f;
  Mes1->Ny=(BYTE)temp<<2;
  temp=(CompMes1->Buffer[3]) & 0xc0;
  Mes1->Ny+=(BYTE)temp>>6;

  //Ki
  temp=(CompMes1->Buffer[3]) & 0x3f;
  Mes1->Ki=temp<<14;
  temp=(CompMes1->Buffer[4]) & 0xff;
  Mes1->Ki+=temp<<6;
  temp=(CompMes1->Buffer[5]) & 0xfc;
  Mes1->Ki+=temp>>2;

  //C2
  temp=(CompMes1->Buffer[5]) & 0x03;
  Mes1->C2=(BYTE)temp<<5;
  temp=(CompMes1->Buffer[6]) & 0xf8;
  Mes1->C2+=(BYTE)temp>>3;

  //C3
  temp=(CompMes1->Buffer[6]) & 0x07;
  Mes1->C3=(WORD)temp<<8;
  temp=(CompMes1->Buffer[7]) & 0xff;
  Mes1->C3+=(WORD)temp;

  //C4
  temp=(CompMes1->Buffer[8]) & 0xff;
  Mes1->C4=(WORD)temp<<3;
  temp=(CompMes1->Buffer[9]) & 0xe0;
  Mes1->C4+=(WORD)temp>>5;

  //PB
  temp=(CompMes1->Buffer[9]) & 0x10;
  Mes1->PB=(BYTE)temp>>4;

  Mes1->CSum=CompMes1->CRC;
}

void DeCompressMessage2(Message2 *Mes2, CompMessage2 *CompMes2)
{
  DWORD temp;
  memset(Mes2, 0, sizeof(Message2));

  // Num
  Mes2->Num=(CompMes2->Buffer[0]>>4) & 0x0f;

  //Kbd
  temp=(CompMes2->Buffer[0]) & 0x0f;
  Mes2->Kbd=(BYTE)temp<<1;
  temp=(CompMes2->Buffer[1]) & 0x80;
  Mes2->Kbd+=(BYTE)temp>>7;

  //Ksd
  temp=(CompMes2->Buffer[1]) & 0x7c;
  Mes2->Ksd=(BYTE)temp>>2;

  //t1
  temp=(CompMes2->Buffer[1]) & 0x03;
  Mes2->T1=temp<<24;
  temp=(CompMes2->Buffer[2]) & 0xff;
  Mes2->T1+=temp<<16;
  temp=(CompMes2->Buffer[3]) & 0xff;
  Mes2->T1+=temp<<8;
  temp=(CompMes2->Buffer[4]) & 0xff;
  Mes2->T1+=temp;

  // CRC
  Mes2->CSum=CompMes2->CRC;
}

void DeCompressMessage3(Message3 *Mes3, CompMessage3 *CompMes3)
{
  DWORD temp;
  memset(Mes3, 0, sizeof(Message3));

  // Num
  Mes3->Num=(CompMes3->Buffer[0]>>4) & 0x0f;

  //alpha
  temp=(CompMes3->Buffer[0]) & 0x0f;
  Mes3->Alpha=(WORD)temp<<9;
  temp=(CompMes3->Buffer[1]) & 0xff;
  Mes3->Alpha+=(WORD)temp<<1;
  temp=(CompMes3->Buffer[2]) & 0x80;
  Mes3->Alpha+=(WORD)temp>>7;

  //ti
  temp=(CompMes3->Buffer[2]) & 0x7f;
  Mes3->Ti=temp<<13;
  temp=(CompMes3->Buffer[3]) & 0xff;
  Mes3->Ti+=temp<<5;
  temp=(CompMes3->Buffer[4]) & 0xf8;
  Mes3->Ti+=temp>>3;

  //P
  temp=(CompMes3->Buffer[4]) & 0x07;
  Mes3->P=(BYTE)temp<<3;
  temp=(CompMes3->Buffer[5]) & 0xe0;
  Mes3->P+=(BYTE)temp>>5;

  //tau
  temp=(CompMes3->Buffer[5]) & 0x1f;
  Mes3->Tau=(WORD)temp<<5;
  temp=(CompMes3->Buffer[6]) & 0xf8;
  Mes3->Tau+=(WORD)temp>>3;

  //CI
  temp=(CompMes3->Buffer[6]) & 0x06;
  Mes3->CI=(BYTE)temp>>1;

  //fn
  temp=(CompMes3->Buffer[6]) & 0x01;
  Mes3->fn=(WORD)temp<<12;
  temp=(CompMes3->Buffer[7]) & 0xff;
  Mes3->fn+=(WORD)temp<<4;
  temp=(CompMes3->Buffer[8]) & 0xf0;
  Mes3->fn+=(WORD)temp>>4;

  //dF
  temp=(CompMes3->Buffer[8]) & 0x0f;
  Mes3->dF=(WORD)temp<<6;
  temp=(CompMes3->Buffer[9]) & 0xfc;
  Mes3->dF+=(WORD)temp>>2;

  //df
  temp=(CompMes3->Buffer[9]) & 0x01;
  Mes3->df=(WORD)temp<<13;
  temp=(CompMes3->Buffer[10]) & 0xff;
  Mes3->df+=(WORD)temp<<5;
  temp=(CompMes3->Buffer[11]) & 0xf8;
  Mes3->df+=(WORD)temp>>3;
  //Mes3->df=Mes3->df*(-1);
  if((CompMes3->Buffer[9]) & 0x02)
  {
	 Mes3->df=Mes3->df;
  }
  else
  {
	  Mes3->df=Mes3->df*(-1);
  }

  // kfm
  temp=(CompMes3->Buffer[11]) & 0x07
    ;
  Mes3->Kfm=(BYTE)temp;

  // Nck
  temp=(CompMes3->Buffer[12]) & 0xff;
  Mes3->Nck=(WORD)temp<<6;
  temp=(CompMes3->Buffer[13]) & 0xfc;
  Mes3->Nck+=(WORD)temp>>2;

  // kam
  temp=(CompMes3->Buffer[13]) & 0x03;
  Mes3->Kam=(BYTE)temp<<1;
  temp=(CompMes3->Buffer[14]) & 0x80;
  Mes3->Kam+=(BYTE)temp>>7;

  //kcm
  temp=(CompMes3->Buffer[14]) & 0x70;
  Mes3->Kcm=(BYTE)temp>>4;
  
  Mes3->CSum=CompMes3->CRC; 
}

void DeCompressMessage4(Message4 *Mes4, CompMessage4 *CompMes4)
{
  DWORD temp;
  memset(Mes4, 0, sizeof(Message4));

  // Num
  Mes4->Num=(CompMes4->Buffer[0]>>4) & 0x0f;

  //alpha
  temp=(CompMes4->Buffer[0]) & 0x0f;
  Mes4->Alpha=(WORD)temp<<9;
  temp=(CompMes4->Buffer[1]) & 0xff;
  Mes4->Alpha+=(WORD)temp<<1;
  temp=(CompMes4->Buffer[2]) & 0x80;
  Mes4->Alpha+=(WORD)temp>>7;

  //ti
  temp=(CompMes4->Buffer[2]) & 0x7f;
  Mes4->Ti=temp<<13;
  temp=(CompMes4->Buffer[3]) & 0xff;
  Mes4->Ti+=temp<<5;
  temp=(CompMes4->Buffer[4]) & 0xf8;
  Mes4->Ti+=temp>>3;

  //P
  temp=(CompMes4->Buffer[4]) & 0x07;
  Mes4->P=(BYTE)temp<<3;
  temp=(CompMes4->Buffer[5]) & 0xe0;
  Mes4->P+=(BYTE)temp>>5;

  //tau
  temp=(CompMes4->Buffer[5]) & 0x1f;
  Mes4->Tau=(WORD)temp<<5;
  temp=(CompMes4->Buffer[6]) & 0xf8;
  Mes4->Tau+=(WORD)temp>>3;

  //CI
  temp=(CompMes4->Buffer[6]) & 0x06;
  Mes4->CI=(BYTE)temp>>1;

  //fn
  temp=(CompMes4->Buffer[6]) & 0x01;
  Mes4->fn=(WORD)temp<<12;
  temp=(CompMes4->Buffer[7]) & 0xff;
  Mes4->fn+=(WORD)temp<<4;
  temp=(CompMes4->Buffer[8]) & 0xf0;
  Mes4->fn+=(WORD)temp>>4;

  //dF
  temp=(CompMes4->Buffer[8]) & 0x0f;
  Mes4->dF=(WORD)temp<<6;
  temp=(CompMes4->Buffer[9]) & 0xfc;
  Mes4->dF+=(WORD)temp>>2;

  //df
  temp=(CompMes4->Buffer[9]) & 0x01;
  Mes4->df=(WORD)temp<<13;
  temp=(CompMes4->Buffer[10]) & 0xff;
  Mes4->df+=(WORD)temp<<5;
  temp=(CompMes4->Buffer[11]) & 0xf8;
  Mes4->df+=(WORD)temp>>3;

  temp=(CompMes4->Buffer[9]) & 0x02;
  temp=temp>>1;
  if(!temp)
  {
	  Mes4->df=Mes4->df*(-1);
  }
  else
  {

  }


  // kfm
  temp=(CompMes4->Buffer[11]) & 0x07;
  Mes4->Kfm=(BYTE)temp;

  // Nck
  temp=(CompMes4->Buffer[12]) & 0xff;
  Mes4->Nck=(WORD)temp<<6;
  temp=(CompMes4->Buffer[13]) & 0xfc;
  Mes4->Nck+=(WORD)temp>>2;

  // kam
  temp=(CompMes4->Buffer[13]) & 0x03;
  Mes4->Kam=(BYTE)temp<<1;
  temp=(CompMes4->Buffer[14]) & 0x80;
  Mes4->Kam+=(BYTE)temp>>7;

  //kcm
  temp=(CompMes4->Buffer[14]) & 0x70;
  Mes4->Kcm=(BYTE)temp>>4;
  
  //f1
  temp=(CompMes4->Buffer[14]) & 0x0f;
  Mes4->f1=(WORD)temp<<9;
  temp=(CompMes4->Buffer[15]) & 0xff;
  Mes4->f1+=(WORD)temp<<1;
  temp=(CompMes4->Buffer[16]) & 0x80;
  Mes4->f1+=(WORD)temp>>7;

  //f2
  temp=(CompMes4->Buffer[16]) & 0x7f;
  Mes4->f2=(WORD)temp<<6;
  temp=(CompMes4->Buffer[17]) & 0xfc;
  Mes4->f2+=(WORD)temp>>2;

  //f3
  temp=(CompMes4->Buffer[17]) & 0x03;
  Mes4->f3=(WORD)temp<<11;
  temp=(CompMes4->Buffer[18]) & 0xff;
  Mes4->f3+=(WORD)temp<<3;
  temp=(CompMes4->Buffer[19]) & 0xe0;
  Mes4->f3+=(WORD)temp>>5;
  
  //f4
  temp=(CompMes4->Buffer[19]) & 0x1f;
  Mes4->f4=(WORD)temp<<8;
  temp=(CompMes4->Buffer[20]) & 0xff;
  Mes4->f4+=(WORD)temp;

  //f5
  temp=(CompMes4->Buffer[21]) & 0xff;
  Mes4->f5=(WORD)temp<<5;
  temp=(CompMes4->Buffer[22]) & 0xf8;
  Mes4->f5+=(WORD)temp>>3;

  Mes4->CSum=CompMes4->CRC; 
}


